package edu.csusm.spotrobot;

import com.sun.spot.peripheral.Spot;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.peripheral.ISwitch;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.iaroc.irobot.util.KeyedStorage;
import com.sun.spot.sensorboard.io.IOutputPin;

/**
 * The SpotBot class holds the main algorithms to control the robot.
 *
 * @author Zebulon Evans
 * @author Clark Scheff
 *
 */
public class SpotBot {
    
    /** Speed of the robot ranges from 0-127 */
    public static int CRUISESPEED           = 80;
    /** The number of cm where Earl is too close to the front wall */
    public static final int TOOCLOSE        = 13;
    /** The number of cm where Earl is too close to the side wall */
    public static final int CLOSE        = 6;
    /** The number of cm where Earl is too far from the side wall */
    public static final int FAR        = 20;
    
    /** The identifier for the left front ir sensor */
    public static final int LEFT_FRONT      = 0;
    /** The identifier for the left rear ir sensor */
    public static final int LEFT_REAR       = 2;
    
    /** Number of ticks to make a 90 degree rotate */
    public static final int ROTATE_90   = 17;
    /** Number of ticks to make a 180 degree rotate */
    public static final int ROTATE_180  = 34;
        
    /** Flag indicating if debug messages will be printed or not */
    private static final boolean DEBUG = true;
    
    /** Used to sense if a 38kHz signal is ahead of the robot */
    private BeaconSensor beaconSensor;
    /** Side infrared sensor */
    private IrSensor irSensors;
    /** Front sonar sensor */
    private SonarSensor sonarSensor;
    /** Controls the servo to change the direction of the sonar sensor */
    private SonarServo sonarServo;
    /** Get how far the bot has travelled */
    private OdometrySensor odoSensor;
    /** Used to send commands to control the motors */
    private MotorController motors = new MotorController();
    
    /** The sensor board on the SunSPOT that has the switches, LEDs, and serial */
    private EDemoBoard demo = EDemoBoard.getInstance();
    /** The first switch on the SunSPOT */
    private ISwitch sw1 = demo.getSwitches()[0]; 
    /** The second switch on the SunSPOT */
    private ISwitch sw2 = demo.getSwitches()[1]; 
    /** The LEDs on the SunSPOT */
    private ITriColorLED[] leds = demo.getLEDs();

    /** Set high to run, if brought low will resets motor controller and the PIC */
    private IOutputPin resetPin;
    
    /** Used to hold the readings from the IR sensors and sonar sensor */
    private float leftFront, leftRear, front;
    /** 
     * Last reading from the left IR sensor. Used for when Earl has to turn
     * left and go forward two feet into the next 2'x2' grid space. lastLeft
     * allows Earl to go forward about the same amount into the grid space 
     * based on how far he was from the wall last time he read the wall. 
     */
    private float lastLeft = 0;
    /** Used to remember if the robot has detected the 38kHz modulated IR */
    private boolean detectedBeacon = false;
    
    /**
     * Initialize the spotbot here
     * D0 is rx D1 is tx for UART
    */
    public SpotBot(){
        // Diabled this because the SPOT was going into a deep sleep when it was
        // removed from USB power. This may not be the case with the blue SDK.
        Spot.getInstance().getSleepManager().disableDeepSleep();
        Spot.getInstance().getSleepManager().disableDiagnosticMode();
        
        // set LED color
        for( int i = 0; i < 8; i++){
            leds[i].setRGB( 25, 0, 50);
        }     
        
        // initialize the UART for serial communication to PIC
        demo.initUART(38400, false);  // baud_rate, two_stop_bits
        
        // setup the reset pin
        IOutputPin[] oPins = demo.getOutputPins();
        resetPin = oPins[EDemoBoard.H3];
        resetPin.setHigh();
        
        toSleep(1000);
        
        sonarSensor = new SonarSensor(EDemoBoard.D3);
        odoSensor = new OdometrySensor();
        irSensors = new IrSensor();
        //sonarServo = new SonarServo(EDemoBoard.D2);
        beaconSensor = new BeaconSensor(EDemoBoard.D4);
        irSensors.start();
        
        toSleep(500);

        motors.sendCommand( MotorController.SET_TICK_SPEED, this.CRUISESPEED );
        run();
    }
    
    /**
     * Main logic loop.
     */
    public void run(){
        
        while(sw1.isOpen()){
            toSleep(80);
           
            // uncomment this and comment mazeExplorer if you want to run the
            // algorithm to find a 38kHz modulated IR (iRobot Roomba Base
            // Station)
            //this.findIr();
            this.mazeExplorer();
            
        }   
    }
    
    /**
     * Empty function to run arbitrary tests
     */
    private void test(){
        //System.out.println("reading " + irSensors.getReading(LEFT_REAR));
        //testSonarReading();
        //this.testIr();
        //this.setLedDifference( irSensors.getReading( LEFT_FRONT ) - irSensors.getReading( LEFT_REAR) );
    }
    
    /**
     * Algorithm to explore the maze as setup for the first annual iaroc.org 
     * competition. Earl will follow the left wall until he finds the IR beacon
     * at which point he will go forward a little more to try to bump the switch
     * attached to the IR beacon to finish the maze.
     */
    private void mazeExplorer(){
        this.updateIR();
        float fullFrontal = sonarSensor.getReading();
        debug("Entering mazeExplorer!");
        debug("leftFront="+leftFront+" leftRear="+leftRear+" fullFrontal="+fullFrontal);
               
        if( fullFrontal <= this.TOOCLOSE )
        {
            if( beaconSensor.detect() && detectedBeacon == false ){
                float numTicks = fullFrontal;
                if( numTicks < 1 ) numTicks = 1;
                motors.sendCommand( MotorController.STRAIGHT, (int)numTicks );
                detectedBeacon = true;
                return;
            } else {
                detectedBeacon = false;
            }
            debug("We are right in front of a wall, rotating 90 degrees CW");
            // if we are here that means we are next to a wall and
            // there is a wall within 13cm or less of us so rotate right
            motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90 );
            lastLeft = leftFront;
            return;
        }
        
        // there is a wall to the left
        if(leftFront <= this.FAR && leftRear <= this.FAR)
        {
            if(leftFront <= this.CLOSE || leftRear <= this.CLOSE)
            {
                while(!motors.sendCommand( MotorController.ROTATE_RIGHT_N, this.ROTATE_90));   
                while(!motors.sendCommand( MotorController.STRAIGHT, 6 ));
                while(!motors.sendCommand( MotorController.ROTATE_LEFT_N, this.ROTATE_90));
            }
            debug("Left IR sensors are both sensing a wall.");
            if( fullFrontal >= 70 )
            {
                debug("Sonar does not see a wall within 2 feet of us.");
                debug("We are moving forward 2 feet.");
                //next to a wall and nothing in front so move forward 2'
                this.alignWithWall();
                motors.sendCommand(MotorController.STRAIGHT, 68);
            }
            else if(fullFrontal > this.TOOCLOSE)
            {
                debug("I'm next to a wall so I'll move forward just a bit");
                // we're next to a wall but there is a wall in front of us 
                // so we'll move up to about 10cm away from it.
                this.alignWithWall();
                fullFrontal = sonarSensor.getReading();
                System.out.println("fullFrontal: " + fullFrontal);
                float numTicks = ((fullFrontal - 12)/(float)0.911);
                if( numTicks < 1 ) numTicks = 1;
                motors.sendCommand( MotorController.STRAIGHT, (int)numTicks );
            }
            
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront >= this.FAR && leftRear <= this.FAR)
        {
            debug("The left front sensor is picking up an opening but the rear");
            debug("still sees a wall, moving forward a bit");
            motors.sendCommand( MotorController.STRAIGHT, 20);
            
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront <= this.FAR && leftRear >= this.FAR)
        {
            debug("The left rear sensor is picking up an opening but the front");
            debug("still sees a wall, moving forward a bit");
            motors.sendCommand( MotorController.STRAIGHT, 12);
            
            lastLeft = leftFront;
            return;
        }
        
        if(leftFront >= this.FAR && leftRear >= this.FAR)
        {
            debug("There is no wall beside us so turn left and move 2 feet");
            while(!motors.sendCommand( MotorController.ROTATE_LEFT_N, ROTATE_90 ));
            // 22 ticks is approximately 20 cm
            fullFrontal = sonarSensor.getReading();
            debug("leftFront >= 25");
            // we thought there was no wall, but it ends up we just got away too far
            if(fullFrontal <= 40)
            {
                while(!motors.sendCommand( MotorController.STRAIGHT, (int)fullFrontal - 10));
                while(!motors.sendCommand( MotorController.ROTATE_RIGHT_N, ROTATE_90-2));
            }
            else
            {
                int ticks = 32;
           
                if(lastLeft > 0)
                    ticks += (((int)lastLeft)*2);
                else
                    ticks += 20;
                debug("ticks: " + ticks);
                while(!motors.sendCommand( MotorController.STRAIGHT, ticks + 2));
            }
            
            lastLeft = leftFront;
            if(lastLeft > 15)
                lastLeft = 0;
            
            return;
        }
    }
    
    /**
     * Behavior to spin until the beacon is detected and go towards it.
     */
    private void findIr(){
        if( !beaconSensor.detect()  ){
            // spin slowly until we find an IR reading
            int rotateCommand = getRotateCommand();
            while( !beaconSensor.detect() ){
                motors.sendCommand( rotateCommand, 30);
            }
            // found the IR keep spinning until we loose it
            int rotateCounter = 0;
            int rotateN;
            if( lastRotate == motors.ROTATE_LEFT )
                rotateN = motors.ROTATE_LEFT_N;
            else
                rotateN = motors.ROTATE_RIGHT_N;
            while( beaconSensor.detect() ){
                motors.sendCommand( rotateN, 1);
                rotateCounter++;
            }
            // lost the IR rotate back half what we rotated and go toards it
            if( lastRotate == motors.ROTATE_LEFT )
                rotateN = motors.ROTATE_RIGHT_N;
            else
                rotateN = motors.ROTATE_LEFT_N;
            motors.sendCommand( rotateN,(int)(rotateCounter/2) + 1);
        }
        
        //  go for IR until we loose it
        motors.sendCommand( motors.STRAIGHT, 10 );
     
    }
    
    int lastRotate = motors.ROTATE_LEFT;
    private int getRotateCommand(){
        if( lastRotate == motors.ROTATE_LEFT ){
            lastRotate = motors.ROTATE_RIGHT;
            return motors.ROTATE_RIGHT;
            
        }
        else{
            lastRotate = motors.ROTATE_LEFT;
            return motors.ROTATE_LEFT;
        }
    }
    
    /**
     * Use the side IR sensors to align Earl with the left wall.  Earl will
     * rotate until the readings from the sensors are less than 0.1cm away from
     * each other or until he has rotated left, then right, then left a couple
     * of times at which point if he does not achieve less than 0.1cm difference
     * he calls it close enough and moves on.
     */
    private void alignWithWall(){
       debug("I'm aligning with wall");
       boolean aligned = false;
       int alignThreshold = 0;
       updateIR();
       float diff = leftFront - leftRear;
       if( diff >= 0 && diff <= 1 ) return;
       boolean rotatingRight = false;
       boolean rotatingLeft = false;
       
       while( !aligned ){
        updateIR();
        diff = leftFront - leftRear;
        if( diff >= 0 && diff <= 1 ) return;
        if( Math.abs( leftFront - leftRear ) > 0.1 ){
            if( leftFront > leftRear ){
                // if we were rotatingRight and now need to rotateLeft it is close enough so exit
                if( rotatingRight ){
                    alignThreshold++;
                    if( alignThreshold > 1 ){
                        aligned = true;
                        continue;
                    }
                }
                 
                // front is further out than rear
                // rotate left
                motors.sendCommand(MotorController.ROTATE_LEFT_N, 1);
                rotatingLeft = true;
           
            } else {
                
                if( rotatingLeft ){
                    alignThreshold++;
                }
                 
                // front is closer than rear
                // rotate right
                motors.sendCommand(MotorController.ROTATE_RIGHT_N, 1);
                rotatingRight = true;
            }
            toSleep(120);
        } else {
            aligned = true;
        }
       }
       
    }
    
    
    // HELPER FUNCTIONS
    
    /**
     * Update the leftFront and leftRear variables with readings from the IR
     * sensors.
     */
    private void updateIR(){
        leftFront = irSensors.getReading( LEFT_FRONT );
        leftRear = irSensors.getReading( LEFT_REAR );
        
        // ugly hack to to make Earl align facing a little out from the wall
        leftRear = leftRear + 0.2f;
        
        debug("leftFront: " + leftFront + "leftRear: " + leftRear + "Sonar: " + front);
    }
    
    /**
     * Print a debugging message to System.out if the DEBUG flag is true.
     */
    private void debug( String message ){
        if( DEBUG )
            System.out.println( message );
    }
    
    /**
     * Sleep without having to wrap it in a try/catch.  This is not the best way
     * clean this up.
     */
    private void toSleep( int msec ){
        // TODO: handle try catch higher up so we don't have to do this.
        try{
            Thread.sleep(msec);
        } catch (InterruptedException e) {
            //We've been interrupted: exit the thread
            return;
        }
    }
    
    // TESTS BELOW
    private void testMotorController(){
        motors.sendCommand( MotorController.FORWARD, CRUISESPEED );
        toSleep(2000);
       
    }
    
    private void testIr( ){
        System.out.println( "LEFT_FRONT: " + irSensors.getReading( LEFT_FRONT ) + "cm" + " LEFT_REAR: " + irSensors.getReading( LEFT_REAR ) + "cm");
    }
    
    private void testSonarServo( ){
        float test = 0.5f;
        for( float f = 0.0f; f <= 1.0f; f += 0.2f){
           sonarServo.setOrientation(test);
            System.out.println("SonarServo frontCenter moved to: " + test );
            toSleep(2000);
        }
    }
    
    private void testSonarReading( ){
        float reading = sonarSensor.getReading();
        System.out.println("Sonar frontCenter: " + reading + "cm");
    }
}
